System and method for stochastically predicting the future states of a vehicle

ABSTRACT

A method for predicting future states of a vehicle including the steps of selecting a model having n states reflecting dynamic features of the vehicle; inputting noisy sensor measurements representing a current state of the vehicle to generate (2n+1) sigma points X i  where i=0, . . . . 2n, each of the sigma points having n states; performing (2n+1) integrations, each integration includes propagating the n-states of the respective sigma points X i  through the non-linear function Y i =f(X i ); and combining the propagated sigma points to generate the predicted future states of the vehicle.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present relates generally to a method and system for predicting afuture state of a vehicle and, more particularly, to such a systemutilizing the Unscented Transform (UT) and Numerical Integration (NI)together to stochastically predict the future state.

2. Description of the Related Art

Various vehicular collision detection approaches exist. Some examplesinclude intelligent parking assist and pre-crash sensing. Productionsystems use radars, laser radars, cameras, or ultrasonic sensors todetect obstacles. However, the majority of these systems do not providean assessment of another vehicle's future path of travel, but insteadsolely rely on the proximity of the vehicle as sensed by the sensor.

In order to predict the future position of a vehicle it is conventionalto utilize a mathematical nonlinear model of the vehicle. Conventionalmodels include the constant acceleration kinematic model (CA), thekinematic unicycle model (KU), the kinematic bicycle model (KB) or theclassic bicycle model (CB). Each model consists of differentialequations which, when solved, represent the dynamic action of theautomotive vehicle.

Once the model has been selected, one previously utilized approach wasto utilize Kalman Prediction to predict the future horizon position ofthe vehicle at time T_(o)+T_(h) where T_(h) equals the horizon timeoffset into the future from the current time T_(o). Since all of themodels are nonlinear, continuous time models, in order to apply thediscrete Kalman equations, the nonlinear continuous time models mustfirst be linearized through derivation of the Jacobian state transitionmatrix, ∇F, and the input gain matrix, ∇G. In addition, KalmanPrediction requires that a discrete time system model propagate forwardthrough the prediction horizon T_(h). Therefore, at each propagationstep, T_(step), the linearized, continuous-time system must bediscretized as follows:

$\begin{matrix}{{\left. {x(t)} \right.\sim{\nabla{{Fx}(t)}}} + {\nabla{{Gu}(t)}}} \\ \Downarrow \\{{x\left\lbrack {k + 1} \right\rbrack} = {{A_{d}{x\lbrack k\rbrack}} + {B_{d}{u\lbrack k\rbrack}}}}\end{matrix}$where x(t) is the continuous state, x[ ] is the discretized state, A_(d)is an n×n matrix, B_(d) is an n×p matrix, n is the number of states, pis the number of inputs, and A_(d) and B_(d) are the discretized systemusing the sample time T_(step).

While Kalman Prediction has proven sufficiently accurate in automotivesystems for predicting the future position of the vehicle, KalmanPrediction is necessarily computationally intensive. Sincemicroprocessors of the type used in automotive vehicles, for costconsiderations, are not fast relative to personal computers, thecomputational-intensive equations required by Kalman Prediction mandaterelatively long time steps T_(step) between sequential equations. This,in turn, can introduce error into the predicted future position of thevehicle.

The UT is a method for calculating the statistics of a random variablewhich undergoes a nonlinear transformation. The intuition behind the UTis that it is easier to approximate a Gaussian distribution than it isto approximate an arbitrary nonlinear function or transformation. Incontrast, the Extended Kalman Filter (EKF) approximates a nonlinearfunction using linearization, and this can be insufficient when theselected model has large nonlinearities over short time periods.

The UT has been applied to the Kalman-filtering problem to form thewell-known Unscented Kalman Filter (UKF). This involves a simpleaugmentation of the state to include the noise variables. Subsequently,the process and measurement covariance matrices are included in thecovariance matrix of the augmented state.

U.S. 2008/0071469, published Mar. 20, 2008 to Caveney (a co-inventor ofthe present invention) discloses an alternative approach to using Kalmanprediction. This publication describes predicting a future position ofan automotive vehicle through NI of a non-linear model. NumericalIntegration for Future Vehicle Path Prediction by co-inventor Caveneyalso describes a method for predicting a future position of anautomotive vehicle through NI of a non-linear model. The U.S.2008/0071469 publication, specifically the NI techniques describedtherein, is hereby incorporated in its entirety by reference.

SUMMARY OF THE INVENTION

The inventors discovered that using NI alone to predict the future pathof a vehicle is problematic because the non-linear models of vehiculardynamics are not perfect and sensor measurements are noisy. UT accountsfor the uncertainty of the model and noisy sensor measurements.

Thus, it is an object of the present invention to use UT and NI togetherto stochastically predict the future states of the vehicle (e.g., thevehicle's position, orientation, velocity, yaw-rate, and acceleration).This invention allows for the prediction routine to maintain nonlinearmodels and, consequently, provide more accurate predictions. That themean of the prediction obtained using UT and NI together was moreaccurate than obtained by NI alone was unexpected. Furthermore, theinvention has been shown to be computationally faster than priorprediction methods.

It is another object of the present invention to predict the futurestates of a vehicle onboard the vehicle or using roadside hardware.

It is a further object of the present invention to implement theprediction methodology in collision detection and avoidance systems.

BRIEF DESCRIPTION OF THE DRAWINGS

A more complete appreciation of the invention and many of the attendantadvantages thereof will be readily obtained as the same becomes betterunderstood by reference to the following detailed description whenconsidered in connection with the accompanying drawings, wherein:

FIG. 1 is a flow chart illustrating the UT-NI processing according tothe present invention;

FIG. 2A is a block diagrammatic view of an on-board vehicle stateprediction system according to a first embodiment of the presentinvention;

FIG. 2B illustrates the field of view of the on-board vehicle stateprediction system illustrated in FIG. 2A;

FIG. 3A is a block diagrammatic view of a roadside vehicle stateprediction system according to an embodiment of the present invention;

FIG. 3B illustrates the field of view of the roadside vehicle stateprediction system illustrated in FIG. 3A;

FIG. 4A is a block diagrammatic view of an on-board vehicle stateprediction system according to a second embodiment of the presentinvention;

FIG. 4B illustrates communications between a roadside collisiondetection and avoidance system and vehicles having the on-board vehiclestate prediction system illustrated in FIG. 4A;

FIG. 5A is a block diagrammatic view of an on-board vehicle stateprediction system according to a third embodiment of the presentinvention;

FIG. 5B illustrates communications between vehicles having the on-boardvehicle state prediction system illustrated in FIG. 5A;

FIG. 6 is a flow chart illustrating collision detection and avoidanceprocessing according to the present invention.

FIGS. 7A-7E diagrammatically illustrate a process for allocatingresponsive measures to avoid a collision;

FIG. 7F is a flow chart illustrating the process for allocatingresponsive measures to avoid a collision; and

FIGS. 8A-8F illustrate a comparative analysis of UT-NI to Kalman'smethods for predicting the future states of a vehicle.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

Referring now to the drawings, wherein like reference numerals designateidentical or corresponding parts throughout the several views. FIG. 1 isa flow chart illustrating the processing steps for predicting the futurestates of a vehicle. Description herein will be directed to anautomobile. However, this invention is applicable to other movingobjects which are suitable for non-linear modeling.

In step 101, a non-linear model for the vehicle is chosen. Manyvehicle-level models exist. The transformation between the vehiclecoordinate frame and the earth coordinate frame implies that all modelswill at least include some trigonometric nonlinearities. Any model thatinvolves yaw and lateral motion of the vehicle will introduce furthernonlinear behavior.

The model described herein is the CB model. However, other dynamicmodels which can be utilized include the CA, KU, and KB models. Eachmodel consists of differential equations which, when solved, representthe dynamic action of the vehicle. The CB model includes as vehicleparameters the mass M, yaw inertia JZ, and the perpendicular distancesfrom the front and rear axles to the vehicles center of gravity, a andb, respectively. Thus a+b is the wheelbase of the vehicle. The equationsare,

${\overset{.}{x}}_{CB} = {\begin{bmatrix}\overset{.}{x} \\\overset{.}{y} \\\overset{.}{\psi} \\\overset{.}{\upsilon_{x}} \\\overset{.}{\upsilon_{y}} \\\overset{.}{\omega}\end{bmatrix} = \begin{bmatrix}{{\upsilon_{x}{\cos(\psi)}} - {\upsilon_{y}{\sin(\psi)}}} \\{{\upsilon_{x}{\sin(\psi)}} + {\upsilon_{y}{\cos(\psi)}}} \\w \\{a_{x}\lbrack 0\rbrack} \\{f_{1}\left( {\upsilon_{x},\upsilon_{y},\omega,{\mathbb{d}{w\lbrack 0\rbrack}}} \right)} \\{f_{2}\left( {\upsilon_{x},\upsilon_{y},w,{\mathbb{d}{w\lbrack 0\rbrack}}} \right)}\end{bmatrix}}$ with${f_{1}\left( {\upsilon_{x},\upsilon_{y},w,{\mathbb{d}w}} \right)} = {{\begin{matrix}{\frac{{- 2}\left( {C_{af} + C_{ar}} \right)\upsilon_{y}}{M\;\upsilon_{x}} +} \\{\left( {\frac{{- 2}\left( {{C_{af}a} - {C_{ar}b}} \right)}{M\;\upsilon_{x}} - \upsilon_{x}} \right)w}\end{matrix}{f_{2}\left( {\upsilon_{2},\upsilon_{y},w,{\mathbb{d}w}} \right)}} = \begin{matrix}\begin{matrix}\begin{matrix}{{+ \left( \frac{2\; C_{af}}{M} \right)}{\mathbb{d}w}\mspace{14mu}{and}} \\{\frac{{- 2}\left( {{C_{af}a} - {C_{ar}b}} \right)\upsilon_{y}}{J_{z}\upsilon_{x}} +}\end{matrix} \\{{\left( \frac{{- 2}\left( {{C_{af}a^{2}} + {C_{ar}b^{2}}} \right)}{J_{z}\upsilon_{x}} \right)w} +}\end{matrix} \\{\left( \frac{2C_{af}a}{J_{z}} \right){\mathbb{d}w}}\end{matrix}}$where C_(af) and C_(ar), are the front and rear tire corneringstiffnesses, respectively. The states x, y, and Ψ are with respect tothe earth-fixed coordinate frame and v_(x), v_(y), and a_(x) are withrespect to the vehicle-fixed coordinate frame. x is the longitude withpositive East, y is latitude with positive North, and Ψ is the vehicleheading positive counterclockwise from the x axis. VX is thelongitudinal velocity of the vehicle, and v_(y) is the lateral velocity.ω is the yaw-rate of the vehicle. The inputs to the model are thevehicle longitudinal acceleration a_(x) and the front-wheel angle, d_(w)and are assumed constant over the prediction horizon. All state valuesat the initial time of each prediction (i.e., t=0) are available fromdirect vehicle or Differential Global Positioning System (DGPS)measurements.

The CB model contains a small set of vehicle parameters and differentialequations, while still capturing a large part of the vehicle-leveldynamics. Note that this model does not include tire deformation or tirecaster effects. Only a linear relationship between cornering stiffnessand lateral wheel force is incorporated in the CB model.

FIG. 2 illustrates a simplified block diagram of an embodiment of avehicle state prediction system 201. The system includes amicroprocessor 211. The microprocessor receives inputs from two sets ofsensors. The input information is usually available over a vehiclecommunication bus. Sensors (1 to m) 215, 217, and 219 are provided onthe vehicle and include for example a speed sensor, an accelerationsensor, and a steering wheel angle sensor. Sensors (1 to n) 203, 205,and 207 are autonomous sensors provided on the vehicle to sense othervehicles and include for example a radar, a laser radar, a camera, andan ultrasonic sensor. A Global Positioning System (GPS) and/or aninertial positioning system can be used to provide position andorientation data 209 of the vehicle. The prediction methodology of thepresent invention can be employed without the position and orientationdata 209.

In step 103 a, measurements from sensors 209, 215, 217, and 219 areinput to the microprocessor 211. The measurements are noisy (uncertain).Updated sensor measurements are obtained every 100 milliseconds, forexample. See step 103 b. The present invention accounts for theuncertainty of the measurements as disclosed below. See also StochasticPath Prediction using the Unscented Transform with NumericalIntegration, by co-inventor Caveney.

In step 105, an n-dimensional random variable x with mean x andcovariance P_(x) is propagated through a nonlinear function y=f (x). Tocalculate the statistics of y, the UT generates 2n+1 deterministicpoints (known as sigma points) X_(i) with corresponding weights W_(i).The sigma points are defined as:x ₀ = xx _(i)= x+(√{square root over ((n+λ)P _(x))})_(i) i=1, . . . , nx _(i)= x−(√{square root over ((n+λ)P _(x))})_(i−n) i=n+1, . . . , 2nwhere (√{square root over ((n+λ)P_(x))})_(i) is the ith row (or column)of the matrix square root. These sigma points are propagated throughnonlinear function,y _(i) =f(x _(i)) i=0, . . . , 2n.See step 107 of FIG. 1.

According to an embodiment of the present invention, when combining NIand UT for path prediction, the propagation through the nonlinearfunction will be performed by the Runge Kutta-Fehlburg (RKF) equations.Also known as the Embedded Runge-Kutta formulas, this adaptive step-sizemethod is attractive because it allows fifth order accuracy with onlysix function evaluations. However, other NI techniques including theclassical fourth-order Runge-Kutta equations can be utilized.

For the six-state classic bicycle model, the propagation will require2(6)+1=13 integrations. In step 109, the propagated sigma points arecombined into a stochastic prediction. The stochastic prediction isrepresented in a time or spaced based parameterization in step 111. Themean, y, and covariance, P_(y) of y are approximated by a weightedsample mean and covariance of the propagated sigma points, Y_(i),

$\begin{matrix}{\overset{\_}{y} \approx {\sum\limits_{i = 0}^{2n}{W_{i}^{(m)}y_{i}}}} \\{P_{y} \approx {\sum\limits_{i = 0}^{2n}{{W_{i}^{(c)}\left( {y_{i} - \overset{\_}{y}} \right)}\left( {y_{i} - \overset{\_}{y}} \right)^{T}}}}\end{matrix}$Where the weights are defined by

$\begin{matrix}{W_{o}^{(m)} = \frac{\lambda}{n + \lambda}} \\{W_{o}^{(c)} = {\frac{\lambda}{n + \lambda} + \left( {1 - \alpha^{2} + \beta} \right)}} \\{W_{i}^{(m)} = {W_{L}^{(c)} = {\frac{1}{2\left( {n + \lambda} \right)}.}}}\end{matrix}$Here λ=α² (n+k)−n is a scaling α parameter. α determines the spread ofthe sigma points around x and is typically set to 1e−3. κ is a secondaryscaling parameter which is usually set to 0, and β is used toincorporate prior knowledge of the distribution of x (for Gaussiandistributions, a value of 2 for β is optimal).

As discussed above, the UT has been applied to the Kalman-filteringproblem to form the well-known UKF. This involves a simple augmentationof the state to include the noise variables. Subsequently, the processand measurement covariance matrices are included in the covariancematrix of the augmented state. However, in the present invention,predictions are only performed where there is no measurement update orprocess noise, so the UT is only applied to the stochastic state, x. Bycombining NI with UT, both a better estimate and associated covariancecan be obtained.

As illustrated in FIG. 2B, the autonomous sensors 203, 205, and 207 havea field of view 221 which enable Vehicle A to sense and predict thefuture states of other vehicles. The field of view 221 is depicted forsimplicities sake in FIG. 2B as being circular. However, each of theautonomous sensors will have different fields of view.

As depicted in FIG. 2B, Vehicle A is able to sense Vehicles B, D, E, andF. However, Vehicle C is not within the sensors field of view 221. Inaddition to predicting its own future states using sensors 209, 215,217, and 219, Vehicle A is able to predict the future states of VehiclesB, D, E, and F using the output of autonomous sensors 203, 205, and 207as the inputs for the initial sensor measurements, step 103 a, andupdated sensor measurements, step 103 b, discussed with regard to FIG.1.

The vehicle state microprocessor 211 outputs the predicted future statesof Vehicles A, B, D, E, and F to collision detection and avoidancemicroprocessor 213. The microprocessors 211 and 213 utilize a computerreadable storage medium, such as a memory (e.g., ROM, EPROM, EEPROM,flash memory, static memory, DRAM, SDRAM, and their equivalents),configured to control the microprocessors to perform the methods of thepresent invention. The microprocessors, in an alternate embodiment,further include or exclusively include a logic device for augmenting orfully implementing the present invention. Such a logic device includes,but is not limited to, an application-specific integrated circuit(ASIC), a field programmable gate array (FPGA), a generic-array of logic(GAL), and their equivalents. The microprocessors 211 and 213 can beseparate devices or a single processing mechanism.

In an alternative embodiment illustrated in FIGS. 3A and 3B, the sensors203, 205, 207, and microprocessors 211 and 213 are used in a roadsidesystem 301. Similar to the field of view 221 illustrated in FIG. 2B, thesensors 203, 205, and 207 used in a roadside system create a field ofview for sensing vehicles. As reflected in FIG. 3B, the roadside systemcan be strategically placed between highway traffic to monitor more thanone direction of traffic. Here, two fields of view 315 a and 315 b areillustrated. Other locations for the roadside system 3A could includeintersections, overpasses, and the like.

The microprocessor 211 uses the output of sensors 203, 205, and 207 topredict the future states of Vehicles A, B, C, D, E, and F. Thestochasticly predicted states are communicated to microprocessor 213 forcollision detection and avoidance processing. If a collision isdetected, then the roadside system 301 communicates to the vehiclesinvolved calculated avoidance measures. The communications 317 a, 317 b,317 c, 317 d, and 317 e are unidirectional wireless communications inthis embodiment. Supplemental or in lieu of the unidirectional wirelesscommunications, road signs could be used to communicate predictedcollisions.

In another embodiment illustrated in FIGS. 4A and 4B, the sensors 209,215, 217, 219, and microprocessor 211 are used in an onboard system 401provided in Vehicles A, B, and C. Microprocessor 213 is provided in aroadside system.

Each Vehicle A, B, and C stochastically predicts its own future statesand then shares its predictions with the roadside system throughbi-directional wireless communications 403 a, 403 b, and 403 c.Collision detection and avoidance is computed by the roadside systemprocessor 213 and the results shared with the Vehicles A, B, and Cthrough the wireless communications.

In another embodiment illustrated in FIGS. 5A and 5B, the sensors 209,215, 217, 219, and microprocessors 211 and 213 are used in an onboardsystem 501 provided in each of Vehicles A, B, and C.

Each Vehicle A, B, and C stochastically predicts its own future statesand then shares its predictions with the other vehicles throughbi-directional wireless communications 503 a, 503 b, and 503 c.Collision detection and avoidance is computed on board each vehicleusing microprocessors 213.

Thus, it can be seen that the embodiments illustrated in FIGS. 2 and 5require no roadside infrastructure. The FIG. 2 embodiment requires nowireless communications. Combinations of the embodiments illustrated inFIG. 2 or 3 with embodiments illustrated in FIG. 4 or 5 can be realizedwhereby a vehicle that has no communication capability has its stateprediction computed by infrastructure or vehicles with autonomoussensing capabilities, and then the prediction is shared with othervehicles through wireless communications.

Methodologies other than the UT-NI stochastic prediction methods of thepresent invention can be used in the embodiments illustrated by FIGS.2-5. Further, it should be noted that with wireless communications, thecollision detection and avoidance processing is applicable to vehiclesthat are traveling at higher speeds and higher relative distances thanis possible with autonomous sensing alone. Two benefits of wirelesscommunications are the allowable range and field of view to detect acollision before it occurs.

FIG. 6 is a flow chart illustrating a collision detection and avoidanceprocessing methodology used for the embodiment of FIG. 5. Similarvariants are employed for the embodiments of FIGS. 2-4. In steps 601,603, and 605, the future states of the vehicle are predicted. In step607, the predicted states and uncertainties (if determined) of thevehicle are shared with other vehicles. In step 609, the predicted pathsof the vehicle and the other vehicles are compared.

In step 611, if the comparison reveals intersecting paths at the sameinstance in time, then in step 613 that information is used by eachinvolved vehicle to take avoidance measures. If the comparison does notreveal an intersecting path, then the processing loops back to step 607where predicted paths of the vehicle are shared. It should be noted thatdifferent time scales can exist for the different step processesillustrated in FIG. 6.

FIGS. 7A-7E diagrammatically illustrate a process for allocatingresponsive measures to avoid a collision involving a plurality ofvehicles. In FIG. 7A, the predicted future state of a Vehicle A isillustrated. The current state of the vehicle is depicted as arectangle. The predicted future state of the Vehicle A is depicted as a98 percentile prediction ellipse in which 98 percent of the X-squaredprobability distribution of each 2-dimensional predicted position lies.The arrow within the vehicle depicts the net vector sum force of thevehicle.

FIG. 7B illustrates the predicted collision of Vehicle A and a VehicleB. In order to calculate the net vector sum force F needed to beimplemented by each of the vehicles to avoid a collision, in the presentinvention, the first step includes determining the overlap of theellipses. In one embodiment, this process includes connecting the twointersection points of the overlapping ellipses. Then, the force F iscalculated as follows:F=(const.*(1−cos(deltaHeading))*TotalVelocity)/(Relative Distance)². Freflects the responsive force needed by each vehicle to repulse the twoellipses from each other. The greater the selected “const.”, the largerthe calculated F. Thus, for example, if the “const.” is set high, thengreater responsive braking will be applied by Vehicle B.

FIG. 7C illustrates Vehicle A in isolation. Two arrows are illustrated:(i) the predicted orientation of the vehicle and (ii) the calculatedresponsive net vector force F needed to be allocated by Vehicle A toavoid the collision. In the case of Vehicle A, a net vector sum forcesubstantially 90 degrees counter clockwise to the projected orientationof the vehicle is needed to avoid the collision. As can be seen fromFIG. 7B, a net vector sum force substantially 180 degrees relative tothe projected orientation of Vehicle B should be allocated to Vehicle Bto avoid the collision.

FIG. 7D identifies the responsive forces available to Vehicle A toimplement the net vector sum force needed to avoid the collision. Theallocation is platform dependent. For example, if the vehicle isall-wheel drive, then acceleration of all four wheels is available.However, if the vehicle is rear wheel drive, then acceleration of onlythe rear wheels is available. As reflected in FIG. 7D, only the frontwheels of Vehicle A can be allocated a responsive force to orient thevehicle substantially 90 degrees counter clockwise to the predictedorientation of the vehicle. See FIG. 7E which reflects that each of thefront wheels can be allocated a lateral and longitudinal responsiveforce F to orient the Vehicle A as needed.

FIG. 7F is a flow chart illustrating the process for allocatingresponsive measures to avoid a collision. This process can beimplemented on-board the vehicle or using roadside processinginfrastructure. In step 701, the predicted overlap (zone) of thevehicles is determined. In step 703, the intersecting force, F, of thevehicles is calculated. In step 705, the single vehicle response to theforce calculated in step 703 is resolved. In step 707, the behavior ofthe single vehicle is changed via braking, steering, and/or accelerationto avoid the collision.

COMPARATIVE ANALYSIS

The comparative analysis provided below was performed using real-worlddata collected on a 2006 Toyota Prius equipped with a DGPS (using WAAScorrections), a longitudinal accelerometer, a yaw-rate gyro, wheel-speedsensors that are averaged for a vehicle-speed estimate, and asteering-wheel-angle sensor. All sensors besides the DGPS were from theproduction vehicle. For consistency, no additional filtering, besidesthat done by the sensors and ECUs themselves was performed on the datacollected off the vehicle's communication bus. This included the use ofthe noisy automotive-grade longitudinal accelerometer over numericallydifferentiated wheel speeds. The steering wheel angle was proportionallyrelated to the front wheel angles by a constant 19.1:1 ratio.Cornering-stiffness values were found empirically while driving on dryasphalt road surfaces.

A. Prediction Accuracy

When linearizing the CB model for Kalman Prediction, it can belinearized once, using the prediction horizon T_(h), or it can bere-linearized M times over the prediction horizon. Note that for thecomparison discussed below, M is chosen to equal the average number ofnumerical integration timesteps for each of the horizon. Looking at theoverall average number of integration timesteps for all five differentprediction horizons, M equals approximately 5 times the predictionhorizon (i.e., for a 3 sec. prediction horizon, an average of 15integration steps are required). Thus, a sampling time T of 0.2 sec. wasused for Kalman Prediction.

The Earth Coordinate Frame is based on the Universal Transverse Mercator(UTM) System grid. It is a decimal, rectangular grid to which sphericaldegree measurements from GPS signals can be converted using variousreference ellipsoids. In this comparison, the WGS-84 reference ellipsoidwas used. Cartesian UTM positions (x, y) are in meters.

The data was collected from a checkroad at the Toyota Technical Centerin Ann Arbor, Mich. As illustrated in FIG. 8A, the checkroad allows fora variety of driving conditions, from straight-aways to curvy handlingsections. This first plot shows the predicted positions usingKalman-Prediction equations of the linearized CB model for a 3 sec.horizon around the track at moderate city speeds (averaging 55 kph) ondry roads. Also drawn on this plot are the 98 percentile predictionellipses, in which 98 percent of the X-squared probability distributionof each 2-dimensional predicted UTM position lies. For comparison, thesecond plot (FIG. 8B) shows the predicted position and ellipses usingthe unscented transform with numerical integration (UT-NI) of the CBmodel for the same data set. The superiority of the UT-NI approach isimmediately evident.

Table I provides the Root Mean Square (RMS) prediction accuracy fordifferent loops around the checkroad at different speeds. Thesuperiority of the UT-NI approach is particularly apparent during longprediction horizons and high vehicle speeds. It is interesting to notethat means of the prediction position using the UT-NI approach are moreaccurate than the results provided by the NI approach alone, where thenonlinear CB model is simply integrated ahead using the (deterministic)initial condition.

TABLE I RMS POSITION ACCURACY (IN METERS) COMPARISON FOR THE TWOAPPROACHES AND DIFFERENT AVERAGE VEHICLE SPEED Prediction Horizon(seconds) Approach Speed 1 sec 2 sec 3 sec 4 sec 5 sec Kalman 45 kph0.42 1.29 3.79 8.72 15.9 55 kph 1.01 3.32 7.88 16.1 27.9 67 kph 1.494.87 11.4 23.0 40.0 UT-NI 45 kph 0.21 1.25 1.91 3.72 6.07 55 kph 0.321.29 3.29 5.93 8.57 67 kph 0.44 1.52 4.51 7.80 9.96

The checkroad in Ann Arbor receives good GPS satellite visibility. Withthe WAAS differential beacon correction, the absolute position accuracyof the DGPS unit is roughly 1 m. However, the prediction accuracy is afunction of relative accuracy, thus the values in Table I can fall below1 m.

The superior prediction capabilities of the UT-NI approach are furtherillustrated in Table II, where the percentage of actual positions whichfell within the 98 percentile ellipse of their predicted positions isgiven. Again this is shown for three different speeds.

TABLE II PERCENTAGE OF ACTUAL POSITIONS CONTAINED WITHIN 98 PERCENTILEELLIPSE OF THE PREDICTED POSITIONS Prediction Horizon (seconds) ApproachSpeed 1 sec 2 sec 3 sec 4 sec 5 sec Kalman 45 kph 100 98.8 85.0 60.142.1 55 kph 98.4 63.7 42.1 30.9 22.6 67 kph 97.5 59.1 35.0 23.0 16.6UT-NI 45 kph 100 100 97.8 87.0 78.1 55 kph 100 97.9 87.6 76.4 72.1 67kph 100 98.3 83.3 69.3 69.1B. Computation Requirements

Maintaining floating-point-operation counts in MATLAB withmachine-optimized BLAS3 is no longer possible. Fortunately, with moderncomputer architectures, memory references and cache usage dominatefloating-point operations in their effect on execution speed. Therefore,it was decided to use the tic and toc commands available in MATLAB tocalculate execution speeds and to use execution speed as the measure ofcomputation requirements. The comparisons obtained were performed onMATLAB version R2006a using its latest version of LAPACK andLinuxX86-optimized BLAS3 libraries. The platform included a 2.13 GHzprocessor with 2.0 GB RAM and 2 MB cache memory.

Table III shows the average execution times for a loop taken at anaverage speed of 55 kph, while starting and ending at 0 kph. For a givenprediction horizon, the computation times at other speeds werecomparable to these values found at 55 kph.

TABLE III COMPUTATION TIME COMPARISON (IN MILLISECONDS) OF KALMANPREDICTION AND UT-NI APPROACH Prediction Horizon (seconds) ApproachSpeed 1 sec 2 sec 3 sec 4 sec 5 sec Kalman 55 kph 9.7 18.5 27.5 36.445.0 UT-NI 55 kph 23.1 36.9 49.1 63.8 87.7

From Table III, each additional second of prediction horizon addsroughly 9 ms and 14 ms to the computation time of the Kalman and UT-NIapproaches, respectively. Also, it should be noted that the computationtimes of the UT-NI approach are roughly 7 times that of the times givenby the NI approach although 13 Runge-Kutta evaluations are performed inthe present work per prediction, whereas only 1 Runge Kutta evaluationwas performed in the NI approach of Caveney's U.S. application Ser. No.11/554,150.

Table III also shows a rough factor of two increase in execution timeusing the UT-NI over the Kalman-Prediction approach. However, any singleprediction with up to a 5 second horizon is still executable within 100ms. Furthermore, a particular property of the UT-NI approach notincluded in this comparison, is that the individual sigma points can bepropagated in parallel, where as the Kalman prediction approach requiressequential matrix evaluations and model linearizations/discretizations.A parallel-processing architecture for computing the UT-NI approachshould easily require less computation time than the Kalman approach.That said, parallel-processing versions of BLAS3 and LAPACK do exist(e.g., PBLAS and SLAPACK), and should be equally evaluated to seepossible reductions in the computation time of the Kalman Predictionapproach.

Obviously, numerous modifications and variations of the presentinvention are possible in light of the above teachings. It is thereforeto be understood that within the scope of the appended claims, theinvention may be practiced otherwise than as specifically describedherein.

The invention claimed is:
 1. A method for predicting future states of avehicle by combining an unscented transform (UT) with numericalintegration (NI), the method comprising: selecting a model, by one ormore processors, having n states reflecting dynamic features of thevehicle; inputting, as the UT, noisy sensor measurements representing acurrent state of the vehicle, by one or more processors, to generate(2n+1) sigma points X_(i), where i=0, . . . , 2n, each of the sigmapoints having n states; performing, as the NI, (2n+1) numericalintegrations by one or more processors, each integration includingpropagating the n-states of the respective sigma points X_(i) throughthe non-linear function Y_(i)=f(X_(i)); and combining, by one or moreprocessors, the propagated sigma points to generate the predicted futurestates of the vehicle, wherein the predicted future states of thevehicle are generated only when there is no measurement update orprocess noise so that an unscented transform is applied only to astochastic state.
 2. The method of claim 1, wherein the input sensormeasurements include velocity, yaw-rate, and acceleration.
 3. The methodof claim 1, further comprising the step of determining a position andorientation of the vehicle using at least one of a Global PositioningSystem and an inertial positioning system, wherein the determinedposition and orientation of the vehicle further represent the currentstate of the vehicle.
 4. The method of claim 1, further comprising thestep of updating the sensor measurements, wherein the propagating stepis repeated each time the sensor measurements are updated.
 5. The methodof claim 1, further comprising the step of representing the predictedstates in a time or space based parameterization.
 6. The method of claim1, further comprising: determining a value reflecting an uncertainty ofthe predicted future states.
 7. The method of claim 1, wherein thepredicted futures states define a predicted patch of the vehicle, themethod further comprising: sharing the predicted path of the vehicle,referred to as a first vehicle, with one or more other vehicles,referred to as a second vehicle or second vehicles; comparing thepredicted path of the first vehicle with a predicted path of one or moreof the second vehicles; determining whether the predicted paths at asame instance in time overlap; and controlling the first vehicle to takeavoidance measures.
 8. The method of claim 1, wherein: X_(i) is ann-dimensional random variable with a mean X and a covariance P_(X);X ₀ = XX _(i) = X +(√{square root over ((n+λ)P _(X))})_(i) , i=1, . . . , nX _(i) = X −(√{square root over ((n+λ)P _(X))})_(i−n) , i=n+1, . . . ,2n (√{square root over ((n+λ)P_(X))})_(i) is an i-th row or column of amatrix square root; and λ is a scaling parameter.
 9. The method of claim8, wherein: a mean Y and covariance P_(Y) are approximated by a weightedsample mean and covariance of propagated sigma points Y_(i);${\overset{\_}{Y} \approx {\sum\limits_{i = 0}^{2\; n}{W_{i}^{m}Y_{i}}}};$${P_{Y} \approx {\sum\limits_{i = 0}^{2\; n}{{W_{i}^{c}\left( {Y_{i} - \overset{\_}{Y}} \right)}\left( {Y_{i} - \overset{\_}{Y}} \right)^{T}}}};$${W_{0}^{m} = \frac{\lambda}{n + \lambda}};$${W_{0}^{c} = {\frac{\lambda}{n + \lambda} + \left( {1 - \alpha^{2} + \beta} \right)}};$${W_{i}^{m} = {W_{i}^{c} = \frac{1}{2\left( {n + \lambda} \right)}}};$ αdefines a spread of sigma points around X; and β defines prior knowledgeof a distribution of X.
 10. The method of claim 1, wherein the NI isperformed with the Runge-Kutta-Fehlberg (RFK) equations.
 11. The methodof claim 1, wherein the propagating the n-states of the respective sigmapoints X_(i) through the non-linear function Y_(i)=f(X_(i)) includespropagating, in parallel, individual sigma points by parallel-processingat a same time.
 12. A system for predicting future states of a vehicleby combining an unscented transform (UT) with numerical integration(NI), the system comprising: a first plurality of sensors, each sensorconfigured to output a performance measurement of the vehiclerepresentative of a current state of the vehicle; and a processorconfigured to predict future states of the vehicle based on the sensedperformance measurements, the prediction calculated by the processor bygenerating, as the UT, (2n+1) sigma points X_(i), where i=0, . . . , 2n,each of the sigma points having n states, performing, as the NI, (2n+1)numerical integrations, each integration including propagating then-states of the respective sigma points X_(i) through the non-linearfunction Y_(i)=f(X_(i)), and combining the propagated sigma points togenerate the predicted future states of the vehicle, wherein thepredicted future states of the vehicle are generated only when there isno measurement update or process noise so that an unscentedtransformation transform is applied only to a stochastic state.
 13. Thesystem of claim 12, wherein the plurality of sensors provided on thevehicle sense the velocity, yaw-rate, and acceleration of the vehicle.14. The system of claim 12, further comprising means for determining aposition and orientation of the vehicle, wherein the determined positionand orientation of the vehicle further represent the current state ofthe vehicle.
 15. The system of claim 12, further comprising: a secondplurality of sensors configured to sense a current state of at least oneother vehicle; the processor configured to predict whether a collisioninvolving the vehicle and a sensed vehicle will occur in the future, andto control the vehicle to avoid the collision.
 16. The system of claim15, wherein the second plurality of sensors are selected from the groupconsisting of radars, cameras, and ultra-sonic sensors.
 17. The systemof claim 12, further comprising: a wireless communication device fortransmitting the predicted future states of the vehicle with at leastone other vehicle, and for receiving the predicted future states of theat least one other vehicle; the processor configured to predict whethera collision involving the vehicle and the at least one other vehiclewill occur in the future based on the predicted future states of thevehicle and the received predicted future states of the at least oneother vehicle, and to control the vehicle to avoid the collision. 18.The system of claim 12, further comprising: a wireless communicationdevice for transmitting the predicted future states of the vehicle to aroadside device; the roadside device configured to receive the predictedfuture states of the vehicle and at least one other vehicle withintransmission range of the roadside device, to predict whether acollision involving the vehicle and the at least one other vehicle willoccur in the future based on the received predicted future states of thevehicle and the received predicted future states of the at least oneother vehicle, and to transmit instructions to the vehicle and the atleast one other vehicle to avoid the collision.
 19. A roadside systemfor predicting future states of vehicles by combining an unscentedtransform (UT) with numerical integration (NI), the system comprising: aplurality of sensors configured to sense the state of at least onevehicle; and a processor configured to predict future states of eachsensed vehicle based on sensed performance measurements, the predictioncalculated by the processor by generating, as the UT, (2n+1) sigmapoints X_(i), where i=0, . . . , 2n, each of the sigma points having nstates, performing, as the NI, (2n+1) numerical integrations, eachintegration including propagating the n-states of the respective sigmapoints X_(i) through the non-linear function Y_(i)=f(X_(i)), andcombining the propagated sigma points to generate the predicted futurestates of each sensed vehicle, wherein the processor is configured topredict whether a collision involving the sensed vehicles will occur inthe future based on the predicted future states of the sensed vehicles,and to transmit instructions to the sensed vehicles to avoid thecollision, and the predicted future states of the vehicle are generatedonly when there is no measurement update or process noise so that anunscented transform is applied only to a stochastic state.
 20. Anon-transitory computer readable medium having stored thereoninstructions for causing the computer to implement a method forpredicting future states of a vehicle by combining an unscentedtransform (UT) with numerical integration (NI), the method comprising:inputting, as the UT, noisy sensor measurements representing a currentstate of the vehicle to generate (2n+1) sigma points X_(i), where i=0, .. . , 2n, each of the sigma points having n states; performing, as theNI, (2n+1) numerical integrations, each integration includingpropagating the n-states of the respective sigma points X_(i) throughthe non-linear function Y_(i)=f(X_(i)); and combining the propagatedsigma points to generate the predicted future states of the vehicle,wherein the predicted future states of the vehicle are generated onlywhen there is no measurement update or process noise so that anunscented transform is applied only to a stochastic state.